clear;
close all;
clc;
rad=0.2
thre = 0.02;
% thre = 0.35;
% freqs= [300 600 1500 3000]; %just a random frequency
% freqs= [850 1000]; %just a random frequency
freqs= [850 900 1000]; %just a random frequency
fs = 44100;
% whos
plot_each =0;
tdur=40e-3;
projection1 = 0; % neariest center
% projection1 = 1; % first project to the unit circle
Num_centers = 1;
Proto_Num = 1.2;
% Proto_Num = 2.2;
% Proto_Num = 2.40;

true_angle = [-80 -65 -55 -45 -40 -35 -30 -25 -20 -15 -10 -5 0 5 10 15 20 25 30 35 40 45 55 65 80];
Elevation_number = 9;


Angles = 10:10:90;
% Angles = 10:10:80;
conds = 'AB';
sides = {'Left','Right'};

PString = num2str(Proto_Num);
if Proto_Num==2.4
    PString = [num2str(Proto_Num) '0'];
end
for iangle = 1:length(Angles)
    iangle
    cc = 0;
    if plot_each==1
        f=figure('position',[100 70 900 500]);
    end
    for icond = 1:2
        eval(['load All_sound_recordings\Protocol_' PString(1) '\Protocol_' PString '_' sides{icond} '_'   num2str(abs(Angles(iangle))) 'degree.mat']);
        recorded_sound=recorded_sound(fs:end-fs,:);
        left_sound = recorded_sound(:,1);
        right_sound = recorded_sound(:,2);
        for ifreql = 1:length(freqs)
            freq = freqs(ifreql);

            legendall{ifreql}=[num2str(freq) ' Hz'];
            [SL,SR,freq_pos,flag]=mySTFT(left_sound,right_sound,fs,thre,tdur);
%             freq_pos(1:100)'
            % normalization:
            [SL1,SR1]=Normalize(SL,SR);
            [Y Ind] = min(abs(freq_pos - freq));
            X_P = [real(SR1(Ind,flag>0))' imag(SR1(Ind,flag>0))'];

            X_P = f_cleanup_Bnoise(X_P,fs,freq,tdur);
            cc=cc+1;
            if plot_each==1
                figure(f)
                subplot(2,3,cc)
                for i=1:length(X_P)
                    plot(X_P(i,1),X_P(i,2),'k.');hold on;
                end
                deg_sign=char(0176);
                %                         [IDX, C] = kmeans(X_P, Num_centers,'Distance','cityblock','Replicates',5);
            end
            out = f_mycenter(X_P,rad);
            C = out.C1;
            %             eval(['load Freq_centers' num2str(freq) ])
            % eval(['load Freq_centers_hardware' num2str(freq) ])
            eval(['load Freq_centers_own' num2str(freq) ])

            if plot_each==1
                for i=1:length(F_centers)
                    s=plot(F_centers(i,1),F_centers(i,2),'go');hold on;set(s,'linewidth',2)
                end
                ss=text(0.6,-0.8,[num2str(freq) ' Hz']);set(ss,'fontsize',14,'color','r')

                plot(C(1,1),C(1,2),'bo','Markersize',20,'LineWidth',2);hold on;
                grid on
                set(gca,'Xtick',-1:0.5:1)
                set(gca,'Ytick',-1:0.5:1)
                axis([-1 1 -1 1])
                hold on;
                plot(-1:1,[0,0,0],'k', 'LineWidth', 0.5);
                plot([0,0,0],-1:1,'k','LineWidth',0.5)
                set(gca,'fontsize',10)
                xlabel('Re','fontsize',12)
                ylabel('Im','fontsize',12)
                hold on
                % now plot the line:
                C_new = C./sqrt(C(1)^2+C(2)^2);
                plot([0 C_new(1)],[0 C_new(2)],'r');
                plot(x_spiral,y_spiral,'color',[0.5 0.5 0.5]) ; % nturns crossings, including end point
                axis square
            end

            %             out = f_angle_class_center(C,F_centers,projection1); % cluster centers
            out = f_spiral_classification(C,x_spiral,y_spiral);
            fine_angles = linspace(true_angle(1), true_angle(end), length(x_spiral));



            %             classified_angle= true_angle(out.class)
            classified_angle= fine_angles(out.class);
            record_class{icond}(ifreql,iangle)=classified_angle;
            if plot_each==1
                if icond==1 & ifreql == 1
                    t = text(-2.8, 0.5, ['True Loc: ' num2str(-Angles(iangle)) ])
                elseif icond==2 & ifreql == 1
                    t = text(-2.8, 0.5, ['True Loc: ' num2str(Angles(iangle)) ])
                end
                set(t,'fontsize',12, 'FontWeight','bold')
                title(['Classified Loc:' num2str(round(classified_angle))])
            end
            X_sp_freq(ifreql,:)=x_spiral;
            y_sp_freq(ifreql,:)=y_spiral;
            C_freq(ifreql,:)=C;
        end % freq

        outMul = f_spiral_classification_MultiF(C_freq,X_sp_freq,y_sp_freq);
        fine_angles = linspace(true_angle(1), true_angle(end), length(x_spiral));

        classified_MultiF= fine_angles(outMul.class)
        record_classM{icond}(iangle)=classified_MultiF;

        pause(0.01)
    end
end


figure('position',[100 70 650 300]);
subplot(1,2,1)
pc = 'bmg';
for ifreq = 1:length(freqs)
    s=plot([fliplr(-Angles) Angles],[fliplr(record_class{1}(ifreq,:)) record_class{2}(ifreq,:)]',pc(ifreq));hold on;
    set(s,'linewidth',2)
end
s=plot([-100 100],[-100 100],'k');set(s,'linewidth',1)
legend(legendall,'Location','northwest')
legend boxoff
xlabel('True Angle (degrees)','fontsize',12)
ylabel('classified Angle (degrees)','fontsize',12)
title('Single Source')
axis square


subplot(1,2,2)
s=plot([fliplr(-Angles) Angles],[fliplr(record_classM{1}) record_classM{2}]','r');hold on;
set(s,'linewidth',2)

s=plot([-100 100],[-100 100],'k');set(s,'linewidth',1)
legend('Combined Freqs','Location','northwest')
legend boxoff
xlabel('True Angle (degrees)','fontsize',12)
ylabel('classified Angle (degrees)','fontsize',12)
title('Single Source')
axis square
